# -*- coding: utf-8 -*-
from MyTelloLib.Gesture import Gesture
from MyTelloLib.TelloCV import TelloCV
# import joystick
from MyTelloLib.MyTello import MyTello
# import video
import cv2
# import threading
import numpy as np
import time

if __name__=="__main__":
    Tello=MyTello('192.168.10.2')
    telloCV=TelloCV(Tello)
    telloCV.upper=(30,255,255)
    telloCV.lower=(12,80,150)
    telloCV.init()
    Tello.flight.motor_on()
    # Tello.flight.takeoff()
    h = Gesture()
    oldCmd=None
    cmd=None
    # Tello.get_video_frame()
    # cap = cv2.VideoCapture(0)
    # time.sleep(1)
    while True:
        # ret,frame=cap.read()
        frame=Tello.get_video_frame()
        
        
        # img=telloCV.face_process_frame(frame) #处理面部追踪
        # img=cv2.cvtColor(img,cv2.COLOR_BGR2RGB) #回转画面颜色
        
        img=telloCV.ball_process_frame(frame) #小球追踪
        
        hand_jugg, gesture_str ,img = h.detect(frame) #手势识别
        # print (gesture_str)
        if gesture_str == 'Thumb,Up':
            cmd='takoff'
            if cmd !=oldCmd:
                oldCmd=cmd
                Tello.flight.takeoff()
                print ('||||||||||||||||||||||||||||||||||||||||||||||')
        if gesture_str == 'Pink,Up':
            cmd='land'
            if cmd !=oldCmd:
                oldCmd=cmd
                Tello.flight.land()
                print ('----------------------------------------------------')
        
            
        ##########################小球追踪调试#########################
        # hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)  # 转换为HSV空间
        # #  消除噪声
        # mask = cv2.inRange(hsv,orange_lower, orange_upper)  # 设定掩膜取值范围
        # opening = cv2.morphologyEx(mask, cv2.MORPH_OPEN, np.ones((5, 5), np.uint8))  # 形态学开运算
        # #bila = cv2.bilateralFilter(mask, 10, 200, 200)  # 双边滤波消除噪声
        # edges = cv2.Canny(opening, 50, 100)  # 边缘识别
        # circles = cv2.HoughCircles(
        #     edges, cv2.HOUGH_GRADIENT, 1, 100, param1=100, param2=10, minRadius=5, maxRadius=100)
        # if circles is not None:  # 如果识别出圆
        #     for circle in circles[0]:
        #         x = int(circle[0])
        #         y = int(circle[1])
        #         radius = int(circle[2])
        #         center = (x, y)
    
        # cv2.imshow('hsv', mask)
        # cv2.imshow('rgb',frame)
        ###############################小球追踪调试###########################
        
        cv2.imshow('result',img)
        if cv2.waitKey(1)&0xFF==27:
            break
    # cap.release()
    cv2.destroyAllWindows()
    # Tello.sock.close()
    # Tello.flight.land()
    Tello.flight.motor_off()
    Tello.quit()
    # Tello.drone.close()
    del telloCV
    del Tello